#include "asuro.h"

int value = 80;
unsigned int lData[2];
float r = 0.4;

void calibrate() {
	int hell;
	int dunkel;
	StatusLED(GREEN);
	Msleep(3000);
	StatusLED(YELLOW);
	Msleep(2000);
	StatusLED(RED);
	LineData(lData);
	hell = (int) lData[LEFT];
	Msleep(1000);
	StatusLED(GREEN);
	Msleep(3000);
	StatusLED(YELLOW);
	Msleep(2000);
	StatusLED(RED);
	dunkel = lData[LEFT];
	Msleep(1000);
	value = (int) ((hell + dunkel) / 2);
	for (int i = 5; i > 0; i--) {
		StatusLED(GREEN);
		Msleep(i * 100);
		StatusLED(RED);
		Msleep(i * 100);
	}
	StatusLED(YELLOW);
}
/*
void driveBad() {
	LineData(lData);
	int leftLux = (int) lData[LEFT];
	if (leftLux > value) {
		StatusLED(RED);
		SetMotorPower(100, (int) 100 * r);
	} else {
		StatusLED(GREEN);
		SetMotorPower((int) 100 * r, 100);
	}
}
*/
void driveGood(int speed) {
	LineData(lData);
	unsigned int speedL =speed;
	unsigned int speedR =speed;
	unsigned int left = lData[LEFT];
	unsigned int right =  lData[RIGHT];
	if(left<right){
		speedR=speedR/(right-left);
	}else{
		speedL=speedL/(left-right);
	}
	MotorDir(FWD,FWD);
	MotorSpeed(speedR,speedL);
}

int main(void) {
	Init();
	FrontLED(ON);
	calibrate();
	while (1) {
		driveGood(150);
	}
return 0;
}
